﻿using System;
using System.Runtime.InteropServices;

namespace PFD
{
    public interface ICompass
    {
        /// <summary>
        /// Compass heading.
        /// </summary>
        double Angle { get; }
        /// <summary>
        /// Angle is valid
        /// </summary>
        bool IsValid { get; }
        /// <summary>
        /// Start compass sensor
        /// </summary>
        void Start();
        /// <summary>
        /// Stop compass sensor
        /// </summary>
        void Stop();
        /// <summary>
        /// Reset compass sensor
        /// </summary>
        void Reset();
    }

    public class HTCCompass : ICompass
    {
        private IntPtr devHandle;
        private int srvHandle;

        public short[] buffer = new short[5];

        #region ICompass

        public double Angle
        {
            get
            {
                // try to connect if necessary
                if (!ready) ready = initCompass();
                return ready && updateData() ? buffer[3] : -1.0;
            }
        }

        bool ready = false;
        public bool IsValid { get { return ready; } }
        public void Start() { ready = initCompass(); }
        public void Stop() { shutdownCompass(); ready = false; }
        public void Reset() { Stop(); Start(); }

        #endregion

        private const int DIO_INIT = 0x3000001;
        private const int DIO_QUIT = 0x3000002;
        private const int DIO_READ = 0x3000003;
        private const long INVALID_HANDLE_VALUE = -1L;
        //private const int LMEM_FIXED = 0;
        //private const int LMEM_ZEROINIT = 0x40;
        //private const int LPTR = 0x40;

        public bool initCompass()
        {
            if (this.devHandle != IntPtr.Zero)
                return true;

            this.devHandle = CreateFile("SEN0:", 0, 0, IntPtr.Zero, 3, 0x80, IntPtr.Zero);
            if ((this.devHandle.ToInt32() == INVALID_HANDLE_VALUE) | (this.devHandle.ToInt32() == 0))
            {
                this.devHandle = IntPtr.Zero;
                this.srvHandle = RegisterService("SEN", 0, "HTCSensorService.dll", 1);
                if (this.srvHandle == 0)
                    return false;

                this.devHandle = CreateFile("SEN0:", 0, 0, IntPtr.Zero, 3, 0x80, IntPtr.Zero);
                if ((this.devHandle.ToInt32() == INVALID_HANDLE_VALUE) | (this.devHandle.ToInt32() == 0))
                    return false;
            }
            int read = 0;
            if (DeviceIoControl(this.devHandle, DIO_INIT, IntPtr.Zero, 0, IntPtr.Zero, 0, out read, IntPtr.Zero) == 0)
                return false;
            return true;
        }

        public bool shutdownCompass()
        {
            if (this.devHandle != IntPtr.Zero)
            {
                int read = 0;
                if (DeviceIoControl(this.devHandle, DIO_QUIT, IntPtr.Zero, 0, IntPtr.Zero, 0, out read, IntPtr.Zero) == 0)
                    return false;
                this.devHandle = IntPtr.Zero;
            }
            return true;
        }

        public override string ToString()
        {
            return ("X: " + buffer[0].ToString() +
            "\r\nY: " + buffer[1].ToString() +
            "\r\nZ: " + buffer[2].ToString() +
            "\r\nA: " + buffer[3].ToString() +
            "\r\nS: " + buffer[4].ToString() +
            "\r\n");
        }

        public bool updateData()
        {
            byte[] buf = new byte[10];
            IntPtr getMemory = new IntPtr();
            if ((this.devHandle == IntPtr.Zero) | (this.devHandle == IntPtr.Zero))
            {
                return false;
            }
            getMemory = LocalAlloc(0x40, buf.Length);
            Marshal.Copy(buf, 0, getMemory, buf.Length);
            int read = 0;
            int result = DeviceIoControl(this.devHandle, DIO_READ, IntPtr.Zero, 0, getMemory, buf.Length, out read, IntPtr.Zero);
            Marshal.Copy(getMemory, buf, 0, buf.Length);
            if (getMemory != IntPtr.Zero)
            {
                LocalFree(getMemory);
            }
            if (result == 0)
            {
                return false;
            }
            Buffer.BlockCopy(buf, 0, buffer, 0, buf.Length);
            return true;
        }

        # region PInvokes

        [DllImport("coredll.dll", CharSet = CharSet.Unicode, SetLastError = true)]
        public static extern IntPtr LocalAlloc(int uFlags, int uBytes);
        [DllImport("coredll.dll", CharSet = CharSet.Unicode, SetLastError = true)]
        public static extern IntPtr LocalFree(IntPtr hMem);
        [DllImport("coredll.dll", CharSet = CharSet.Unicode, SetLastError = true)]
        public static extern int RegisterService(string lpszType, int dwIndex, string lpszLib, int dwInfo);
        [DllImport("coredll.dll", CharSet = CharSet.Unicode, SetLastError = true)]
        public static extern IntPtr CreateFile(string lpFileName, int dwDesiredAccess, int dwShareMode, IntPtr lpSecurityAttributes, int dwCreationDisposition, int dwFlagsAndAttributes, IntPtr hTemplateFile);
        [DllImport("coredll.dll", CharSet = CharSet.Unicode, SetLastError = true)]
        public static extern int DeviceIoControl([In] IntPtr hDevice, [In] int dwIoControlCode, [In] IntPtr lpInputBuffer, [In] int nInBufferSize, [Out] IntPtr lpOutBuffer, [In] int nOutBufferSize, out int lpBytesReturned, [In] IntPtr lpOverlapped);

        #endregion
    }
}
